﻿#include "DroneStateMsg.h"
#include <json/json.h>
#include <vector>

std::ostream & operator<<(std::ostream & os, const DroneStateMsg & droneStateMsg)
{
	os << "droneNumber is:" << droneStateMsg.droneNumber << std::endl;
	os << "position is:(" << droneStateMsg.position[0] << "," << droneStateMsg.position[1] << "," << droneStateMsg.position[2] << ")" << std::endl;
	os << "orientation is:(" << droneStateMsg.orientation[0] << "," << droneStateMsg.orientation[1] << "," << droneStateMsg.orientation[2] << "," << droneStateMsg.orientation[3] << ")" << std::endl;
	os << "velocity is:(" << droneStateMsg.velocity[0] << "," << droneStateMsg.velocity[1] << "," << droneStateMsg.velocity[2] << ")" << std::endl;
	os << "mode is: " << droneStateMsg.mode << std::endl;
	os << "bridgeMessage is: " << droneStateMsg.bridgeMessage << std::endl;
	return os;
}

DroneStateMsg::DroneStateMsg()
{
	droneNumber = -1;
	position = std::vector<double>(3, 0);
	orientation = std::vector<double>(4, 0);
	velocity = std::vector<double>(3, 0);
	mode = "";
	bridgeMessage = "";
}


void DroneStateMsg::getDroneState(const std::string& droneStateMsg)
{
	Json::Reader reader;
	Json::Value data;
	if (reader.parse(droneStateMsg.c_str(), data))
	{
		droneNumber = data["droneNumber"].asInt();
		position[0] = data["positionX"].asDouble();
		position[1] = data["positionY"].asDouble();
		position[2] = data["positionZ"].asDouble();
		orientation[0] = data["orientationX"].asDouble();
		orientation[1] = data["orientationY"].asDouble();
		orientation[2] = data["orientationZ"].asDouble();
		orientation[3] = data["orientationW"].asDouble();
		velocity[0] = data["velocityX"].asDouble();
		velocity[1] = data["velocityY"].asDouble();
		velocity[2] = data["velocityZ"].asDouble();
		mode = data["mode"].asString();
		bridgeMessage = data["bridgeMessage"].asString();
	}

	else {
		std::cout << "convert fails..." << std::endl;

	}
}